#include "BBox.h"
#include <stdio.h>
using namespace geom;

BBox::BBox(Point3D center, float side) : m_center(center), m_side(side)
{
}

const BBox BBox::operator+(const BBox &other)
{
	BBox result(*this);
	result+=other;
	return result;
}

const BBox& BBox::operator+=(const BBox & other)
{
	float maxAxisSide = 0;
	for(int axis =0; axis<3; axis++)
	{
		float myAxisMin = m_center.v[axis] - m_side/2;
		float myAxisMax = m_center.v[axis] + m_side/2;

		float otherAxisMin = other.m_center.v[axis] - other.m_side/2;
		float otherAxisMax = other.m_center.v[axis] + other.m_side/2;
	
		float minAxis = myAxisMin<otherAxisMin?myAxisMin:otherAxisMin;
		float maxAxis = myAxisMax>otherAxisMax?myAxisMax:otherAxisMax;

		float axisCenter = (maxAxis + minAxis)/2.0;
		m_center.v[axis] = axisCenter;
		float axisSide = (maxAxis - minAxis);
		if(axisSide>maxAxisSide)
			maxAxisSide = axisSide;
	}
	m_side = maxAxisSide;
	return *this;
}

bool BBox::contains(const Point3D &p) const
{
    float cx = m_center.x;
    float cy = m_center.y;
    float cz = m_center.z;

    float sideHalf = m_side/2;

    if(p.x < cx-sideHalf || p.x>cx+sideHalf)
        return false;

    if(p.y < cy-sideHalf || p.y>cy+sideHalf)
        return false;

    if(p.z < cz-sideHalf || p.z>cz+sideHalf)
        return false;

    return true;
}

bool BBox::contains(const Triangle &t) const
{
    return ( contains(t.p1) && contains(t.p2) && contains(t.p3) );
}


int BBox::btOutcode(const Vector3D& p,float halfExtent) const
{
	return 
		(p.dx < -halfExtent ? 0x01 : 0x0) |    
		(p.dx >  halfExtent ? 0x08 : 0x0) |
		(p.dy < -halfExtent ? 0x02 : 0x0) |    
		(p.dy >  halfExtent ? 0x10 : 0x0) |
		(p.dz < -halfExtent ? 0x04 : 0x0) |    
		(p.dz >  halfExtent ? 0x20 : 0x0);
}

bool BBox::isTraversedBy(const Ray &ray) const
{
	float param;
	return getCollisionPoint(ray, param);
}

bool BBox::getCollisionPoint(const Ray &ray, float &param) const
{
	Point3D rayFrom = ray.origin();
	Point3D rayTo = ray.atDistance(50000.0);
	float aabbHalfExtent = m_side/2;
	Vector3D source (rayFrom.x - m_center.x, rayFrom.y - m_center.y, rayFrom.z - m_center.z);
	Vector3D target (rayTo.x - m_center.x, rayTo.y - m_center.y, rayTo.z - m_center.z);
	int	sourceOutcode = btOutcode(source,aabbHalfExtent);
	int targetOutcode = btOutcode(target,aabbHalfExtent);
	
	// If the ray starts and ends at the same side of the box in any direction, 
	// it cannot traverse it
	if ((sourceOutcode & targetOutcode) != 0x0) 
		return false;

	float lambda_enter = 0.0;
	float lambda_exit  = 1.0;
	Vector3D r = target - source;
	int i;
	float normSign = 1;
	int bit=1;

	// We check with positive and negative normals (both sides of the cube)
	for (int j=0;j<2;++j)
	{
		// We check each direction
		for (i = 0; i < 3; ++i,bit<<=1)
		{
			if (sourceOutcode & bit)
			{
				float lambda = (-source.dv[i] - aabbHalfExtent*normSign) / r.dv[i];
				if (lambda_enter <= lambda)
				{
					lambda_enter = lambda;
				}
			}
			else if (targetOutcode & bit) 
			{
				float lambda = (-source.dv[i] - aabbHalfExtent*normSign) / r.dv[i];
				lambda_exit = lambda_exit<lambda? lambda_exit : lambda;
			}
		}
		normSign = -1.0;
	}

	if (lambda_enter <= lambda_exit)
	{		param = lambda_enter;
		return true;
	}

	return false;
}


